Object tracking by unsupervised learning

ABSTRACT

A computing device in a vehicle can determine an object by determining a first rectangular bound centered on the object based on joint probabilities of first 3D data points, then determining the object by determining a second rectangular bound centered on the object based on joint Bayesian probabilities of second 3D data points conditioned on the first rectangular bound and piloting the vehicle based on the determined object.

BACKGROUND

Vehicles can be equipped to operate in both autonomous and occupantpiloted mode. Vehicles can be equipped with computing devices, networks,sensors and controllers to pilot the vehicle and to assist an occupantin piloting the vehicle. A computing device can monitor the real-worldenvironment of the vehicle via sensors to assist the computing device inpiloting the vehicle.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a block diagram of an example vehicle.

FIG. 2 is a diagram of a LIDAR sensor and 3D data points.

FIG. 3 is a diagram of a LIDAR sensor and 3D data points.

FIG. 4 is a diagram of a LIDAR sensor and 3D data points.

FIG. 5 is a diagram of a LIDAR sensor and 3D data points.

FIG. 6 is a flowchart diagram of an example process to pilot a vehicle.

FIG. 7 is a flowchart diagram of an example process to pilot a vehicle.

FIG. 8 is a diagram of an enclosing rectangle.

DETAILED DESCRIPTION

Vehicles can be equipped to operate in both autonomous and occupantpiloted mode. By a semi- or fully-autonomous mode, we mean a mode ofoperation wherein a vehicle can be piloted by a computing device as partof a vehicle information system having sensors and controllers. Thevehicle can be occupied or unoccupied, but in either case the vehiclecan be piloted without assistance of an occupant. For purposes of thisdisclosure, an autonomous mode is defined as one in which each ofvehicle propulsion (e.g., via a powertrain including an internalcombustion engine and/or electric motor), braking, and steering arecontrolled by one or more vehicle computers; in a semi-autonomous modethe vehicle computer(s) control(s) one or two of vehicle propulsion,braking, and steering.

Vehicles can be equipped with computing devices, networks, sensors andcontrollers to pilot the vehicle and to determine maps of thesurrounding real world including features such as roads. Vehicles can bepiloted and maps can be determined based on locating and identifyingroad signs in the surrounding real world. By piloting we mean directingthe movements of a vehicle so as to move the vehicle along a roadway orother portion of a path.

FIG. 1 is a diagram of a vehicle information system 100 that includes avehicle 110 operable in autonomous (“autonomous” by itself in thisdisclosure means “fully autonomous”) and occupant piloted (also referredto as non-autonomous) mode in accordance with disclosed implementations.Vehicle 110 also includes one or more computing devices 115 forperforming computations for piloting the vehicle 110 during autonomousoperation. Computing devices 115 can receive information regarding theoperation of the vehicle from sensors 116.

The computing device 115 includes a processor and a memory such as areknown. Further, the memory includes one or more forms ofcomputer-readable media, and stores instructions executable by theprocessor for performing various operations, including as disclosedherein. For example, the computing device 115 may include programming tooperate one or more of vehicle brakes, propulsion (e.g., control ofacceleration in the vehicle 110 by controlling one or more of aninternal combustion engine, electric motor, hybrid engine, etc.),steering, climate control, interior and/or exterior lights, etc., aswell as to determine whether and when the computing device 115, asopposed to a human operator, is to control such operations.

The computing device 115 may include or be communicatively coupled to,e.g., via a vehicle communications bus as described further below, morethan one computing devices, e.g., controllers or the like included inthe vehicle 110 for monitoring and/or controlling various vehiclecomponents, e.g., a powertrain controller 112, a brake controller 113, asteering controller 114, etc. The computing device 115 is generallyarranged for communications on a vehicle communication network such as abus in the vehicle 110 such as a controller area network (CAN) or thelike; the vehicle 110 network can include wired or wirelesscommunication mechanism such as are known, e.g., Ethernet or othercommunication protocols.

Via the vehicle network, the computing device 115 may transmit messagesto various devices in the vehicle and/or receive messages from thevarious devices, e.g., controllers, actuators, sensors, etc., includingsensors 116. Alternatively, or additionally, in cases where thecomputing device 115 actually comprises multiple devices, the vehiclecommunication network may be used for communications between devicesrepresented as the computing device 115 in this disclosure. Further, asmentioned below, various controllers or sensing elements may providedata to the computing device 115 via the vehicle communication network.

In addition, the computing device 115 may be configured forcommunicating through a vehicle-to-infrastructure (V-to-I) interface 111with a remote server computer 120, e.g., a cloud server, via a network130, which, as described below, may utilize various wired and/orwireless networking technologies, e.g., cellular, BLUETOOTH® and wiredand/or wireless packet networks. The computing device 115 also includesnonvolatile memory such as is known. Computing device 115 can loginformation by storing the information in nonvolatile memory for laterretrieval and transmittal via the vehicle communication network and avehicle to infrastructure (V-to-I) interface 111 to a server computer120 or user mobile device 160.

As already mentioned, generally included in instructions stored in thememory and executed by the processor of the computing device 115 isprogramming for operating one or more vehicle 110 components, e.g.,braking, steering, propulsion, etc., without intervention of a humanoperator. Using data received in the computing device 115, e.g., thesensor data from the sensors 116, the server computer 120, etc., thecomputing device 115 may make various determinations and/or controlvarious vehicle 110 components and/or operations without a driver tooperate the vehicle 110. For example, the computing device 115 mayinclude programming to regulate vehicle 110 operational behaviors suchas speed, acceleration, deceleration, steering, etc., as well astactical behaviors such as a distance between vehicles and/or amount oftime between vehicles, lane-change, minimum gap between vehicles,left-turn-across-path minimum, time-to-arrival at a particular locationand intersection (without signal) minimum time-to-arrival to cross theintersection.

Controllers, as that term is used herein, include computing devices thattypically are programmed to control a specific vehicle subsystem.Examples include a powertrain controller 112, a brake controller 113,and a steering controller 114. A controller may be an electronic controlunit (ECU) such as is known, possibly including additional programmingas described herein. The controllers may communicatively be connected toand receive instructions from the computing device 115 to actuate thesubsystem according to the instructions. For example, the brakecontroller 113 may receive instructions from the computing device 115 tooperate the brakes of the vehicle 110.

The one or more controllers 112, 113, 114 for the vehicle 110 mayinclude known electronic control units (ECUs) or the like including, asnon-limiting examples, one or more powertrain controllers 112, one ormore brake controllers 113 and one or more steering controllers 114.Each of the controllers 112, 113, 114 may include respective processorsand memories and one or more actuators. The controllers 112, 113, 114may be programmed and connected to a vehicle 110 communications bus,such as a controller area network (CAN) bus or local interconnectnetwork (LIN) bus, to receive instructions from the computer 115 andcontrol actuators based on the instructions.

Sensors 116 may include a variety of devices known to provide data viathe vehicle communications bus. For example, a radar fixed to a frontbumper (not shown) of the vehicle 110 may provide a distance from thevehicle 110 to a next vehicle in front of the vehicle 110, or a globalpositioning system (GPS) sensor disposed in the vehicle 110 may providegeographical coordinates of the vehicle 110. The distance provided bythe radar or the geographical coordinates provided by the GPS sensor maybe used by the computing device 115 to operate the vehicle 110autonomously or semi-autonomously.

The vehicle 110 is generally a land-based autonomous vehicle 110 havingthree or more wheels, e.g., a passenger car, light truck, etc. Thevehicle 110 includes one or more sensors 116, the V-to-I interface 111,the computing device 115 and one or more controllers 112, 113, 114.

The sensors 116 may be programmed to collect data related to the vehicle110 and the environment in which the vehicle 110 is operating. By way ofexample, and not limitation, sensors 116 may include, e.g., altimeters,cameras, LIDAR, radar, ultrasonic sensors, infrared sensors, pressuresensors, accelerometers, gyroscopes, temperature sensors, pressuresensors, hall sensors, optical sensors, voltage sensors, currentsensors, mechanical sensors such as switches, etc. The sensors 116 maybe used to sense the environment in which the vehicle 110 is operatingsuch as weather conditions, the grade of a road, the location of a roador locations of neighboring vehicles 110. The sensors 116 may further beused to collect dynamic vehicle 110 data related to operations of thevehicle 110 such as velocity, yaw rate, steering angle, engine speed,brake pressure, oil pressure, the power level applied to controllers112, 113, 114 in the vehicle 110, connectivity between components andelectrical and logical health of the vehicle 110.

FIG. 2 is a diagram of a traffic scene 200, where a computing device 115in a vehicle 110 having a LIDAR sensor 202, directs the LIDAR sensor 202to acquire 3D data points 204, 206. 3D data points 204, 206 areclassified into two groups; background 3D data points 204 (closedcircles) and object 3D data points 206 (open circles). 3D data points204, 206 can be automatically labeled as background 3D data points 204or object 3D data points 206 using well-known machine vision techniques.For example, a prior map of 3D data points can be acquired thatrepresents the static or motionless background. Any 3D data points 204,206 acquired after that time that match the static background can belabeled as “background” 3D data points 204 and any 3D data points thatdo not match the static background are labeled “object” 3D data points206.

In traffic scene 200, computing device 115 has determined a firstestimated enclosing rectangle 208 (grey dashed line) based on the object3D data points 206. An enclosing rectangle 800 can be defined as shownin FIG. 8. FIG. 8 is a diagram of an enclosing rectangle 800, definedwith respect to XY coordinate axes 802 as the intersection of two setsof parallel lines 804, 806, 808, 810. The XY coordinate axes 802 can bedefined in a plane parallel to the drivable surface of a roadway, forexample. The XY coordinate axes 802 can be defined to point in alignmentwith geographic directions, with one of the axes pointing north, forexample. In another case, the XY coordinate axes 802 can be defined topoint in alignment with the pose of vehicle 110, with one of the axespointing in the direction of travel of vehicle 110, for example. Ineither case, enclosing rectangle 800 can be defined by location, sizeand pose of the orthogonal pairs of parallel lines 804, 806, 808, 810.

The location of enclosing rectangle 800 can be defined as the XYcoordinates (x₁, y₁) of a vertex point 814, for example, defined inrelation to the XY coordinates (x₀, y₀) of origin point 820 of XYcoordinate axes 802. The origin point 820 can be associated with apredetermined location on vehicle 110, such as a centroid, for example,and therefore the origin point 820 will translate in the XY plane as thevehicle 110 moves on the roadway. The size and pose of enclosingrectangle 800 can be determined based on the XY coordinates (x₁, y₁),(x₂, y₂) of vertex points 812, 814, for example, where the size inlength (L in FIG. 8) and width (W in FIG. 8) can be determined as:

W=x ₂ −x ₁  (1)

L=y ₂ −y ₁  (2)

and pose, defined as an angle (Θ in FIG. 8) with respect to XYcoordinate axes 802 is defined as:

$\begin{matrix}{\Theta = \tan^{{- 1}\frac{W}{L}}} & (3)\end{matrix}$

Returning to FIG. 2, computing device 115 has estimated enclosingrectangle 208 (grey dashed lines) based on object 3D data points 206acquired by LIDAR sensor 202 of true, but unknown to computing device115, object enclosing rectangle 210 (left diagonal hatch line) by usinga rule-based approach that determines parameters for a first enclosingrectangle 208 by first selecting a starting pose for the first enclosingrectangle 208. The starting pose can have the sides of the firstenclosing rectangle 208 parallel to the XY coordinate axes 802respectively, for example. Computing device 115 then iteratively adjuststhe orthogonal pairs of parallel lines to the minimum size required toenclose the maximum number of object 3D data points 206 while enclosingno background 3D data points 204, for example.

The starting pose can be initialized with a heuristic box size from afirst LIDAR scan, or the starting pose can be initialized by determiningbounding boxes as described in the section below entitled “Bounding BoxEstimation”, assigning a “diffuse” or uniform distribution of modelparameters for the prior enclosing rectangle. In this fashion thebounding box estimation algorithm can be informed that we do not haveany information about the bounding boxes, but permits the bounding boxestimation algorithm to find the best conditioned fit from 3D datapoints acquired by the first LIDAR scan.

Prior information known by computing device 115 about a bounding boxsize at the time the 3D data points are acquired by a first LIDAR scancan be used to condition the bounding box estimate. For example, it isknown that objects in traffic lanes rarely exceed the lane width and canbe aligned with the lane. Information regarding an initial priordistribution of model parameters can be encoded into the first boundingbox estimation algorithm iteration.

FIG. 3 is a diagram of traffic scene 300, occurring at a new time step,wherein vehicle 110 has moved, including LIDAR sensor 202. LIDAR sensor202 has provided computing device 115 with a new sample of background 3Ddata points 304 (closed hexagons) and object 3D data points 306 (openhexagons). Computing device 115 has formed a second estimated enclosingrectangle 308 (dashed lines) of true, but unknown, object enclosingrectangle 210 (left diagonal hatch line) by joint Bayesian inferencebased on object 3D data points 306 and first estimated enclosingrectangle 208 as discussed in the section below, entitled “Bounding BoxEstimation”, at the new time step.

The section below, entitled “Bounding Box Estimation”, is a derivationof the calculations required to perform bounding box estimation usingBayesian inference by conditioning the probability distributions p oncurrent and prior observations represented by vector of random variablesZ_(1:k) representing parameters associated with enclosing boxes by theequation:

$\begin{matrix}{{p\left( {\underset{\_}{B},{\underset{\_}{C}}_{k},{\Theta_{k}{\underset{\_}{Z}}_{1\text{:}k}}} \right)} = \frac{\begin{matrix}{{p\left( {{{\underset{\_}{Z}}_{k}\underset{\_}{B}},{\underset{\_}{C}}_{k},\Theta_{k},{\underset{\_}{Z}}_{{1\text{:}k} - 1}} \right)} \cdot {p\left( {\underset{\_}{B}{\underset{\_}{Z}}_{{1\text{:}k} - 1}} \right)} \cdot} \\{\int_{{\underset{\_}{C}}_{k - 1},\Theta_{k - 1}}{p\left( {{\underset{\_}{C}}_{k},\Theta_{k},{\underset{\_}{C}}_{k - 1},{\Theta_{k - 1}\underset{\_}{B}},{\underset{\_}{Z}}_{{1\text{:}k} - 1}} \right)}}\end{matrix}}{p\left( {\underset{\_}{Z}}_{k} \right)}} & (4)\end{matrix}$

also shown as equation (7) below, where p(Z_(k)|B, C_(k), Θ_(k),Z_(1:k-1)) represents the measurement likelihood at time step k,p(B|Z_(1:k-1)) represents the prior time step's box size and ∫_(C)_(k-1) _(,Θ) _(k-1) p(C_(k), Θ_(k), C_(k-1), Θ_(k-1)|B, Z_(1:k-1))represents a stochastic process model that models the joint Bayesianprobability of 3D data point B (location) and enclosing box parametersC_(k), Θ_(k) (size and pose) at time step k and k−1, conditioned on thebox parameters associated with enclosing boxes from previous time steps.These factors are normalized by dividing by p(Z_(k)), the marginalprobability of the enclosing box data at time step k. Appendix A showsthe solution of p(Z_(k)|B, C_(k), Θ_(k), Z_(1:k-1)), the measurementlikelihoods, in the form of joint Bayesian probability distributions, bycalculation of gradients of log-likelihoods and thereby maximizing themeasurement likelihoods.

Maximizing the joint Bayesian probability distributions determines thesecond estimated enclosing rectangle 308 based on the object 3D datapoints 306 and the first estimated enclosing rectangle 208. Note that as3D data points 304, 306 from traffic scene 300 are combined with firstestimated enclosing rectangle 208, second estimated enclosing rectangle308 represents an improved estimate of true, but unknown to computingdevice 115, object enclosing rectangle 210. Note also that, in thiscase, the pose of second estimated enclosing rectangle 308 did notchange from the pose of first estimated enclosing rectangle 208. Thisdoes not have to be the case if the pose of the first estimatedenclosing rectangle 208 did not happen to match the pose of the true,but unknown, enclosing rectangle 210. In this case the pose of thesecond estimated enclosing rectangle 308 will be closer to the pose ofthe true, but unknown, enclosing rectangle 210, than the pose of firstestimated enclosing rectangle 208.

FIG. 4 is a diagram of traffic scene 400, occurring at another new timestep, wherein vehicle 110 has again moved, including LIDAR sensor 202.LIDAR sensor 202 has provided computing device 115 with a new sample ofbackground 3D data points 404 (closed circles) and object 3D data points406 (open circles). Computing device 115 has formed a third estimatedenclosing rectangle 408 (dashed lines) of true, but unknown, objectenclosing rectangle 210 (left diagonal hatch line) based on object 3Ddata points 406 and second estimated enclosing rectangle 308 bymaximizing joint Bayesian probability distributions as discussed abovein relation to FIG. 3 and the section marked “Bounding Box Estimation”,at the new time step.

Note again, that as 3D data points 404, 406 from traffic scene 400 arecombined with previously estimated enclosing rectangle 308, estimatedenclosing rectangle 408 represents an improved estimation of true, butunknown, object enclosing rectangle 210. If the pose of the secondestimated enclosing rectangle 308 did not happen to match the pose ofthe true, but unknown, object enclosing rectangle 210, for example, thepose of the third estimated enclosing rectangle 408 can be closer to thepose of the true, but unknown, object enclosing rectangle 210 than thepose of second estimated enclosing rectangle 308.

FIG. 5 is a diagram of traffic scene 500, occurring at another new timestep, wherein vehicle 110 has again moved, including LIDAR sensor 202.LIDAR sensor 202 has provided computing device 115 with a new sample ofbackground 3D data points 504 (closed circles) and object 3D data points506 (open circles). Computing device 115 has formed a fourth estimatedenclosing rectangle 508 (grey dashed lines) of true, but unknown, objectenclosing rectangle 210 (left diagonal hatch line) based on object 3Ddata points 406 and third estimated enclosing rectangle 406 bymaximizing joint Bayesian probability distributions as discussed abovein relation to FIG. 3 and the section marked “Bounding Box Estimation”,at the new time step.

Note again, that as 3D data points 504, 506 from traffic scene 500 arecombined with previously estimated enclosing rectangle 408, fourthestimated enclosing rectangle 508 again represents an improvedestimation of true, but unknown, object enclosing rectangle 210. If thepose of the third estimated enclosing rectangle 408 did not happen tomatch the pose of the true, but unknown, object enclosing rectangle 210,the pose of the fourth estimated enclosing rectangle 508 can be closerto the pose of the true, but unknown, object enclosing rectangle 210than the pose of third estimated enclosing rectangle 408.

FIG. 6 is a diagram of a flowchart, described in relation to FIGS. 1-5,of a process 600 for piloting a vehicle 110 based on a determinedobject, e.g., as described above. Process 600 can be implemented by aprocessor of computing device 115, taking as input information fromsensors 116, and executing instructions and sending control signals viacontrollers 112, 113, 114, for example. Process 600 includes multiplesteps taken in the disclosed order. Process 600 also includesimplementations including fewer steps or can include the steps taken indifferent orders.

Process 600 begins at step 602 where computing device 115 determines afirst object based on a first plurality of object 3D data points 206.The first object can be determined by determining parameters of a firstestimated enclosing rectangle 208, using a rule-based approach thatdetermines parameters for a first enclosing rectangle 208 by firstselecting a starting pose for the first enclosing rectangle 208 and theniteratively adjusting the orthogonal pairs of parallel lines to theminimum size required to enclose the maximum number of object 3D datapoints 206 while enclosing no background 3D data points 204, asdiscussed above in relation to FIG. 2. Computing device 115 can alsodetermine a starting pose by predetermining model parameters for thefirst iteration as discussed above in relation to FIG. 2.

At step 604 computing device 115 determines a second object bydetermining parameters of a second estimated enclosing rectangle 308based on maximizing joint Bayesian probability distributions based onthe second plurality of object 3D data points 306 and the firstestimated enclosing rectangle 208 as discussed above in relation to FIG.3 and the section marked “Bounding Box Estimation”.

At step 606 computing device 115 pilots vehicle 110 based on the secondobject determined in step 604. Computing device can determine more thanone object in the field of view of LIDAR sensor 202 by maximizing jointBayesian probability distributions using techniques discussed inco-pending application Ser. No. ______, entitled OBJECT TRACKING BYUNSUPERVISED LEARNING and which has the same inventor and is assigned tothe same assignee as this application, which application is fullyincorporated herein by reference in its entirety. Computing device 115can pilot vehicle 110 by predicting paths for objects in the field ofview of LIDAR sensor 202 and then planning a path for vehicle 110 toreach a predetermined location while avoiding the objects. The process600 ends following the block 606.

In summary, process 600 describes a process by which a computing device115 can acquire first object 3D data points 206 using LIDAR sensor 202,determine a first object based on the first object 3D data points,acquire second object 3D data points 306 using LIDAR sensor 202 anddetermine a second object in the field of view of LIDAR sensor 202 basedon the first object and the second object 3D data points and pilotvehicle 110 to a predetermined location while avoiding the object basedon the determined second object.

FIG. 7 is a diagram of a flowchart, described in relation to FIGS. 1-5,of a process 700 for determining an estimated enclosing rectangle basedon 3D data points and previously estimated enclosing rectangles, e.g.,as described above. Process 700 can be implemented by a processor ofcomputing device 115, taking as input information from sensors 116, andexecuting instructions and sending control signals via controllers 112,113, 114, for example. Process 700 includes multiple steps taken in thedisclosed order. Process 700 also includes implementations includingfewer steps or can include the steps taken in different orders.

Process 700 starts at step 702, where computing device 115 comparesacquired 3D data points to previously acquired 3D data points todetermine background 3D data points and object 3D data points asdiscussed above in relation to FIG. 2. At step 704, computing deviceforms joint Bayesian probability distributions conditioned for eachobject 3D data point for each object conditioned on the results of aprevious time step as discussed above in relation to FIG. 3 and inAppendix A.

At step 706, computing device forms a 3×3 grid, aligned with anenclosing rectangle estimated at a previous time step, as shown inTable 1. of the section marked “Bounding Box Estimation”, where thejoint Bayesian probability distributions of the object 3D data pointsare calculated according to class membership probabilities, where object3D data points within the estimated enclosing rectangle are in class 1and object 3D data points occurring outside the estimated enclosingrectangle are in classes 2-9.

At step 708 the estimated rectangular bound is determined by maximizingthe joint Bayesian probability distributions of the object 3D datapoints conditioned on a previous estimated rectangular bound accordingto class membership probabilities on the 3×3 grid as discussed in thesection marked “Bounding Box Estimation”, and at step 710 computingdevice 115 can output the estimated rectangular bounds to a process suchas process 600 to pilot vehicle 110, for example. The process 700 endsfollowing the block 708.

In summary, process 700 is a process for comparing 3D data points topreviously acquired points to determine object 3D data points, formingjoint Bayesian probability distributions based on the object 3D datapoints and conditioned on a previously estimated enclosing rectangle ona 3×3 grid and estimating a new enclosing rectangle based on the object3D data points and a previously estimated enclosing rectangle andoutputting the new estimated enclosing rectangle.

Bounding Box Estimation

Probability distributions p of vectors of random variables B, C_(k),representing parameters associating 3D data points at time step k withenclosing boxes, and random variable Θ_(k) representing parametersassociated with the enclosing boxes at a time step k, can be estimatedusing Bayesian inference to condition the probability distributions p oncurrent and prior observations represented by vector of random variablesZ_(1:k) representing parameters associated with enclosing boxes by theequations:

$\begin{matrix}\begin{matrix}{{p\left( {\underset{\_}{B},{\underset{\_}{C}}_{k},\left. \Theta_{k} \middle| {\underset{\_}{Z}}_{1:k} \right.} \right)} = \frac{{p\left( {\left. {\underset{\_}{Z}}_{k} \middle| \underset{\_}{B} \right.,{\underset{\_}{C}}_{k},\Theta_{k},{\underset{\_}{Z}}_{1:{k - 1}}} \right)} \cdot {p\left( {\underset{\_}{B},{\underset{\_}{C}}_{k},\Theta_{k},\left| {\underset{\_}{Z}}_{1:{k - 1}} \right.} \right)}}{p\left( {\underset{\_}{Z}}_{k} \right)}} \\{= {\frac{\begin{matrix}{p{\left( {\left. {\underset{\_}{Z}}_{k} \middle| \underset{\_}{B} \right.,{\underset{\_}{C}}_{k},\Theta_{k},{\underset{\_}{Z}}_{1:{k - 1}}} \right) \cdot}} \\{{p\left( {\underset{\_}{B},\left| {\underset{\_}{Z}}_{1:{k - 1}} \right.} \right)} \cdot {p\left( {{\underset{\_}{C}}_{k},\left. \Theta_{k} \middle| \underset{\_}{B} \right.,{\underset{\_}{Z}}_{1:{k - 1}}} \right)}}\end{matrix}}{p\left( {\underset{\_}{Z}}_{k} \right)}(6)}} \\{= {\frac{\begin{matrix}{p{\left( {\left. {\underset{\_}{Z}}_{k} \middle| \underset{\_}{B} \right.,{\underset{\_}{C}}_{k},\Theta_{k},{\underset{\_}{Z}}_{1:{k - 1}}} \right) \cdot p}{\left( {\underset{\_}{B},\left| {\underset{\_}{Z}}_{1:{k - 1}} \right.} \right) \cdot}} \\{\int_{{\underset{\_}{C}}_{k - 1},\Theta_{k - 1}}{p\left( {{\underset{\_}{C}}_{k},\Theta_{k},\left. {{\underset{\_}{C}}_{k - 1}\Theta_{k - 1}} \middle| \underset{\_}{B} \right.,{\underset{\_}{Z}}_{1:{k - 1}}} \right)}}\end{matrix}}{p\left( {\underset{\_}{Z}}_{k} \right)}(7)}}\end{matrix} & (5)\end{matrix}$

where p(Z_(k)|B, C_(k), Θ_(k), Z_(1:k-1)) represents the measurementlikelihood at time step k, p(B|Z_(1:k-1)) represents the prior timesteps box size and ∫_(C) _(k-1) _(,Θ) _(k-1) p(C_(k), Θ_(k), C_(k-1),Θ_(k-1)|B, Z_(1:k-1)) represents a stochastic process model that modelsthe joint Bayesian probability of 3D data point B and enclosing boxparameters C_(k), Θ_(k) at time step k and k−1, conditioned on the boxsize and parameters associated with enclosing boxes from previous timesteps. These factors are normalized by dividing by p(Z_(k)), themarginal probability of the enclosing box data at time step k.

Focusing on the measurement likelihood, we first assume each data pointis independent, per the equations:

Z _(k) ∥Z _(l) ∀l≠k  (8)

Z _(i,k) ∥Z _(j,k) ∀i≠j  (9)

where the symbol ∥ means “is statistically independent from”, whichmeans that the model maximum likelihood can be calculated as:

p(Z _(k) |B,C _(k),Θ_(k) ,Z _(1:k-1))=

_(l=1) ^(n) ^(Z) p(Z _(l,k) |B,C _(k),Θ_(k))  (10)

where the

operator means “the product of Bayesian probability distributions” inthis case over 3D data points l from 1 to n_(Z), the number of 3D datapoints. Dropping the time step notation (k) for brevity, look at each 3Ddata point independently, approaching each 3D data point from aclassification perspective, where two sets of parallel lines (bound 1,bound 2 and bound 3, bound 4) arranged orthogonally as a bounding boxform a grid shown in Table 1:

TABLE 1 Bounding box classes and bounds Class6 Class 3 Class 9 Class 4Class 1 Class 5 bound 3 Class 8 Class 2 Class 7 bound 4 bound 2 bound 1

In local coordinates, the boundary conditions for Class 1 are:

$\begin{matrix}{{{\underset{\_}{\underset{\_}{B}}}^{T} = \begin{bmatrix}{{- {\underset{\_}{U}}_{L}^{T}},{{{\underset{\_}{U}}_{L}^{T}\underset{\_}{C}} + {L/2}}} \\{{\underset{\_}{U}}_{L}^{T},{{\left( \underset{\_}{- U} \right)_{L}^{T}\underset{\_}{C}} + {L/2}}} \\{{- {\underset{\_}{U}}_{W}^{T}},{{{\underset{\_}{U}}_{W}^{T}\underset{\_}{C}} + {W/2}}} \\{{\underset{\_}{U}}_{W}^{T},{{{\underset{\_}{- U}}_{W}^{T}\underset{\_}{C}} + {W/2}}}\end{bmatrix}}{where}} & (11) \\{{\underset{\_}{U}}_{L}^{T} = \left\lbrack {{\cos \; \Theta} - {\sin \; \Theta}} \right\rbrack} & (12) \\{{\underset{\_}{U}}_{W}^{T} = \left\lbrack {{\sin \; \Theta},{\cos \; \Theta}} \right\rbrack} & (13)\end{matrix}$

and where row 1 of array (A7) −U_(L) ^(T), U_(L) ^(T)C+L/2=left of bound1, row 2 of array (A7) U_(L) ^(T), −U_(L) ^(T)C+L/2=right of bound 2,row 3 of array (A7) −U_(W) ^(T), U_(W) ^(T)C+W/2=below bound 3 and row 4of array (A7) U_(W) ^(T), −U_(W) ^(T)C+W/2=above bound 4. The classescan be defined in terms of these boundaries by the equation:

$\begin{matrix}{{{\underset{\_}{\underset{\_}{\Theta}}}^{T} = {\underset{\_}{\underset{\_}{\Gamma}} \cdot {\underset{\_}{\underset{\_}{B}}}^{T}}}{where}} & (14) \\{{\underset{\_}{\underset{\_}{\Gamma}} = \begin{bmatrix}1 & 1 & 1 & 1 \\1 & 1 & 1 & {- 1} \\1 & 1 & {- 1} & 1 \\1 & {- 1} & 1 & 1 \\{- 1} & 1 & 1 & 1 \\1 & {- 1} & 1 & 1 \\{- 1} & 1 & 1 & {- 1} \\1 & {- 1} & 1 & {- 1} \\{- 1} & 1 & {- 1} & 1\end{bmatrix}}{and}} & (15) \\{{\underset{\_}{\underset{\_}{\Theta}}}^{T} = {2\begin{bmatrix}{\underset{\_}{O}}_{1 \times 2} & \frac{\left( {L + W} \right)}{2} \\{- {\underset{\_}{U}}_{W}^{T}} & {{{\underset{\_}{U}}_{W}^{T}\underset{\_}{C}} + \frac{L}{2}} \\{\underset{\_}{U}}_{W}^{T} & {{{\underset{\_}{- U}}_{W}^{T}\underset{\_}{C}} + \frac{L}{2}} \\{- {\underset{\_}{U}}_{L}^{T}} & {{{\underset{\_}{U}}_{L}^{T}\underset{\_}{C}} + \frac{W}{2}} \\{\underset{\_}{U}}_{L}^{T} & {{{\underset{\_}{- U}}_{L}^{T}\underset{\_}{C}} + \frac{W}{2}} \\{- \left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)^{T}} & {\left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)^{T}\underset{\_}{C}} \\\left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)^{T} & {{- \left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)^{T}}\underset{\_}{C}} \\{- \left( {{\underset{\_}{U}}_{L} + {\underset{\_}{U}}_{W}} \right)^{T}} & {\left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)^{T}\underset{\_}{C}} \\\left( {{\underset{\_}{U}}_{L} + {\underset{\_}{U}}_{W}} \right)^{T} & {{- \left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)^{T}}\underset{\_}{C}}\end{bmatrix}}} & (16)\end{matrix}$

where the rows of the 9×2 matrix represent classes 1-9 from top tobottom, respectively.

“Soft” boundaries can be determined using a logistic/softmax function:

$\begin{matrix}{{{p\left( {\left. {\underset{\_}{Z}}_{l} \middle| {{\underset{\_}{Z}}_{l} \in {{Class}\mspace{14mu} n}} \right.,\underset{\_}{B},{\underset{\_}{C}}_{k},\Theta_{k}} \right)} = \frac{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}{\sum\limits_{i = 1}^{9}\; {\exp \left( {\lambda {\underset{\_}{\Theta}}_{i}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}}}{{where}\text{:}}} & (17) \\{{{\underset{\_}{\Theta}}_{n}^{T} = {{the}\mspace{14mu} n^{th}\mspace{14mu} {row}\mspace{14mu} {of}\mspace{14mu} {\underset{\_}{\underset{\_}{\Theta}}}^{T}}}{and}} & (18) \\{{\underset{\_}{\overset{\sim}{Z}}}_{l} = \begin{bmatrix}{\underset{\_}{Z}}_{l} \\1\end{bmatrix}} & (19)\end{matrix}$

Now, assume the softmax function accurately labels the object (e.g.class 1) or not (e.g. class 2-9). This motivates forming a “super class”equal to the union of all classes >1.

$\begin{matrix}{{p\left( {\left. {\underset{\_}{Z}}_{l} \middle| {in} \right.,\underset{\_}{B},{\underset{\_}{C}}_{k},\Theta_{k}} \right)} = \frac{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}{\sum\limits_{i = 1}^{9}\; {\exp \left( {\lambda {\underset{\_}{\Theta}}_{i}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}}} & (20) \\{{p\left( {\left. {\underset{\_}{Z}}_{l} \middle| {out} \right.,\underset{\_}{B},{\underset{\_}{C}}_{k},\Theta_{k}} \right)} = \frac{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}{\sum\limits_{i = 1}^{9}\; {\exp \left( {\lambda {\underset{\_}{\Theta}}_{i}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}}} & (21)\end{matrix}$

Returning to model maximum likelihood calculation and substitutingyields:

$\begin{matrix}{{p\left( {\underset{\_}{B},{\underset{\_}{C}}_{k},\left. \Theta_{k} \middle| {\underset{\_}{Z}}_{1:k} \right.} \right)} = \frac{{p\left( \underset{\_}{B} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)} \cdot {p\left( {{\underset{\_}{C}}_{k},\left. \Theta_{k} \middle| \underset{\_}{B} \right.,{\underset{\_}{Z}}_{1:{k - 1}}} \right)} \cdot {\Uparrow_{l = 1}^{n_{Z}}{p\left( {\left. Z_{l,k} \middle| \underset{\_}{B} \right.,{\underset{\_}{C}}_{k},\Theta_{k}} \right)}}}{p\left( {\underset{\_}{Z}}_{k} \right)}} & (22)\end{matrix}$

At this point, two options exist to solve the maximum likelihoodcalculation:

1. Take an EKF (extended Kaman filter) prediction as p (C_(k), Θ_(k)|B,Z_(1:k-1)). Note that in this case, the marginal posterior probabilitydistributions over C_(k) and Θ_(k) cannot be used in the EKF as anobservation because it would be incestuous.

2. Assume a uniform/uninformative/diffuse prior marginal posteriorprobability distributions over C_(k) and Θ_(k). Then we can use themarginal posterior probability distributions as an observation in theEKF calculation.

Assuming option 2:

p(B,C _(k),Θ_(k) |Z _(1:k))≈p(B|Z _(1:k-1))·

_(l=1) ^(n) ^(Z) p(Z _(l,k) |B,C _(k),Θ_(k))  (23)

Adding clustering (object) labels to the notation:

$\begin{matrix}{{p\left( {\underset{\_}{B},{\underset{\_}{C}}_{k},\left. \Theta_{k} \middle| {\underset{\_}{Z}}_{1:k} \right.,{\underset{\_}{Y}}_{k}} \right)} \approx {{p\left( \underset{\_}{B} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)} \cdot \left\{ {\underset{\underset{\Phi_{l}^{IN}}{}}{\left. \Uparrow{}_{l \in {IN}}\frac{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}{\sum\limits_{i = 1}^{9}\; {\exp \left( {\lambda {\underset{\_}{\Theta}}_{i}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}} \right.} \cdot \underset{\underset{\Phi_{l}^{OUT}}{}}{\left. \Uparrow{}_{l \in {OUT}}\frac{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}{\sum\limits_{i = 1}^{9}\; {\exp \left( {\lambda {\underset{\_}{\Theta}}_{i}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}} \right.}} \right\}}} & (24)\end{matrix}$

note that Φ_(l) ^(OUT)=1−Φ_(l) ^(IN)=1−Φ^(l). Restating equation 20using this notation:

p(B,C _(k),Θ_(k) |Z _(1:k) ,Y _(k))≈p(B|Z _(1:k-1))·{

_(l∈IN)Φ_(l)·

_(l∈OUT)(1−Φl)}  (25)

take the natural log:

L=ln [p(B|Z _(1:k-1))]+Σ_(l∈IN) ln(Φ_(l))+Σ_(l∈OUT) ln(1−Φ_(l))  (26)

Now compute the gradient. First, let:

$\begin{matrix}{{\underset{\_}{M} = {\begin{bmatrix}\underset{\_}{B} \\{\underset{\_}{C}}_{k} \\\Theta_{k}\end{bmatrix} = {{set}\mspace{14mu} {of}\mspace{14mu} {model}\mspace{14mu} {parameters}}}}{and}} & (27) \\\left. {\begin{matrix}{\frac{\partial\underset{\_}{L}}{\partial\underset{\_}{M}} = {\underset{}{\frac{\partial}{\partial\underset{\_}{M}}{\ln \left\lbrack {p\left( \underset{\_}{B} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)} \right\rbrack}} + {\Sigma_{l \in {IN}}{\ln \left( \Phi_{l} \right)}} + {\Sigma_{l \in {OUT}}{\ln \left( {1 - \Phi_{l}} \right)}}}} \\{= \begin{bmatrix}{\frac{\partial}{\partial\underset{\_}{B}}{\ln \left\lbrack {p\left( \underset{\_}{B} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)} \right\rbrack}} \\{\underset{\_}{O}}_{2 \times 1} \\O\end{bmatrix}}\end{matrix}{{where}\mspace{14mu} \frac{\partial}{\partial\underset{\_}{B}}{\ln\left\lbrack {p\underset{\_}{B}} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)}}} \right\rbrack & (28)\end{matrix}$

depends upon the form of the polar and can be set so that it is likelyto be “as big” as it was at the previous time step.

Continuing the derivation:

$\begin{matrix}{\frac{\partial}{\partial\underset{\_}{M}} = {{\ln \left( \Phi_{l} \right)} = {\frac{1}{\Phi_{l}} \cdot \frac{\partial\Phi_{l}}{\partial\underset{\_}{M}}}}} & (29) \\{\frac{\partial}{\partial\underset{\_}{M}} = {{\ln \left( {1 - \Phi_{l}} \right)} = {\frac{- 1}{1 - \Phi_{l}} \cdot \frac{\partial\Phi_{l}}{\partial\underset{\_}{M}}}}} & (30)\end{matrix}$

substituting:

$\begin{matrix}{\frac{\partial\underset{\_}{L}}{\partial\underset{\_}{M}} = {{\frac{\partial}{\partial\underset{\_}{M}}{\ln \left\lbrack {p\left( \underset{\_}{B} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)} \right\rbrack}} + {\Sigma_{l \in {IN}}{\frac{1}{\Phi_{l}} \cdot \frac{\partial\Phi_{l}}{\partial\underset{\_}{M}}}} + {\Sigma_{l \in {OUT}}{\frac{- 1}{1 - \Phi_{l}} \cdot \frac{\partial\Phi_{l}}{\partial\underset{\_}{M}}}}}} & (31)\end{matrix}$

where:

$\begin{matrix}{\Phi_{l} = {\frac{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}{\sum\limits_{i = 1}^{9}\; {\exp \left( {\lambda {\underset{\_}{\Theta}}_{i}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)}} = \frac{f_{1}}{\sum\limits_{j = 1}^{9}\; f_{j}}}} & (32)\end{matrix}$

Then, via the quotient rule:

$\begin{matrix}{\frac{\partial\Phi_{l}}{\partial\underset{\_}{M}} = \frac{{\frac{\partial f_{1}}{\partial\underset{\_}{M}} \cdot {\sum\limits_{j = 1}^{9}\; f_{j}}} - {f_{1}{\sum\limits_{j = 1}^{9}\; \frac{\partial f_{j}}{\partial\underset{\_}{M}}}}}{\left\lbrack {\sum\limits_{j = 1}^{9}\; f_{j}} \right\rbrack^{2}}} & (33)\end{matrix}$

and via the chain rule:

$\begin{matrix}{\begin{matrix}{\frac{\partial f_{j}}{\partial\underset{\_}{M}} = {\frac{\partial f_{j}}{\partial\Theta_{j}} \cdot \frac{\partial\Theta_{j}}{\partial\underset{\_}{M}}}} \\{= {\frac{\partial\Theta_{j}^{T}}{\partial{\underset{\_}{M}}^{T}} \cdot \frac{\partial f_{j}}{\partial\Theta_{j}^{T}}}}\end{matrix}{and}} & (34) \\\begin{matrix}{\frac{\partial f_{j}}{\partial\Theta_{j}^{T}} = {\lambda \cdot {\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\underset{\_}{\overset{\sim}{Z}}}_{l}} \right)} \cdot {\underset{\_}{\overset{\sim}{Z}}}_{l}}} \\{= {\lambda \cdot f_{j} \cdot {\underset{\_}{\overset{\sim}{Z}}}_{l}}}\end{matrix} & (36)\end{matrix}$

where

${\frac{\partial f_{j}}{\partial\underset{\_}{M}} \in {\mathbb{R}}^{5 \times 1}},{\frac{\partial f_{j}}{\partial\Theta_{j}^{T}} \in {\mathbb{R}}^{3 \times 1}},{{{and}\mspace{14mu} \frac{\partial\Theta_{j}^{T}}{\partial{\underset{\_}{M}}^{T}}} \in {{\mathbb{R}}^{5 \times 1}.}}$

Substituting 33 into 31 and substituting the result into 29 andsimplifying yields:

$\begin{matrix}{\frac{\partial\Phi_{l}}{\partial\underset{\_}{M}} = {{\lambda\Phi}_{l} \cdot \left\lbrack \frac{\sum\limits_{j = 2}^{9}\; {\left( {{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{\prime} - {\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}} \right)f_{j}}}{\sum\limits_{j = 1}^{9}\; f_{j}} \right\rbrack \cdot {\underset{\_}{\overset{\sim}{Z}}}_{l}}} & (38)\end{matrix}$

and substituting for f_(j):

$\begin{matrix}{= {{\lambda\Phi}_{l} \cdot \left\lbrack \underset{\underset{= {{\underset{\_}{\underset{\_}{\alpha}}}_{l} \in {\mathbb{R}}^{5 \times 3}}}{}}{\frac{\sum\limits_{j = 2}^{9}{\left( {{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{\prime} - {\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}} \right){\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}}}{\sum\limits_{j = 1}^{9}{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}}} \right\rbrack \cdot {\overset{\sim}{\underset{\_}{Z}}}_{l}}} & (39)\end{matrix}$

Substituting back into the gradient of the log-likelihood and cancelingΦ_(l):

$\begin{matrix}{{\frac{\partial\underset{\_}{L}}{\partial\underset{\_}{M}} = {{\frac{\partial}{\partial\underset{\_}{M}}{\ln \left\lbrack {p\left( \underset{\_}{B} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)} \right\rbrack}} + {\lambda \left\lbrack {{\sum\limits_{l \in {IN}}{{\underset{\_}{\underset{\_}{\alpha}}}_{l}{\overset{\sim}{\underset{\_}{Z}}}_{l}}} - {\sum\limits_{l \in {OUT}}{\frac{\varphi_{l}}{1 - \varphi_{l}}{\underset{\_}{\underset{\_}{\alpha}}}_{l}{\overset{\sim}{\underset{\_}{Z}}}_{l}}}} \right\rbrack}}}\mspace{20mu} {{where}\text{:}}} & (40) \\{\mspace{79mu} {{\underset{\_}{\underset{\_}{\alpha}}}_{l} = \frac{\sum\limits_{j = 2}^{9}{d{{\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime} \cdot {\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}}}}{\sum\limits_{j = 1}^{9}{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}}}} & (41)\end{matrix}$

The Θ′ can be calculated as:

$\begin{matrix}\begin{matrix}{{\underset{\_}{\Theta}}_{1}^{\prime} = \begin{bmatrix}0 & 0 & {1/2} \\0 & 0 & {1/2} \\0 & 0 & 0 \\0 & 0 & 0 \\0 & 0 & 0\end{bmatrix}} & {{\underset{\_}{\Theta}}_{2}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & 0\end{matrix} \\\begin{matrix}\begin{matrix}0 & 0 \\0 & 0\end{matrix} & {\underset{\_}{U}}_{W}\end{matrix}\end{matrix} \\\begin{matrix}{- \left( {\underset{\_}{U}}_{W}^{\prime} \right)^{T}} & {\left( {\underset{\_}{U}}_{W}^{\prime} \right)^{T}\underset{\_}{C}}\end{matrix}\end{bmatrix}}}\end{matrix} & (42) \\{{{\underset{\_}{\Theta}}_{3}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & 0\end{matrix} \\\begin{matrix}\begin{matrix}0 & 0 \\0 & 0\end{matrix} & {\underset{\_}{U}}_{W}\end{matrix}\end{matrix} \\\begin{matrix}{- \left( {\underset{\_}{U}}_{W}^{\prime} \right)^{T}} & {{- \left( {\underset{\_}{U}}_{W}^{\prime} \right)^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}}{{\underset{\_}{\Theta}}_{4}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 & 0 \\0 & 0 & {1/2}\end{matrix} \\\begin{matrix}\begin{matrix}0 & 0 \\0 & 0\end{matrix} & {\underset{\_}{U}}_{L}\end{matrix}\end{matrix} \\\begin{matrix}{- \left( {\underset{\_}{U}}_{L}^{\prime} \right)^{T}} & {\left( {\underset{\_}{U}}_{L}^{\prime} \right)^{T}\underset{\_}{C}} \\{- {\underset{\_}{U}}_{W}^{T}} & {{- \left( {\underset{\_}{U}}_{W}^{\prime} \right)^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}}} & (43) \\{{\underset{\_}{\Theta}}_{5}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 & 0 \\0 & 0 & \frac{1}{2}\end{matrix} \\\begin{matrix}\begin{matrix}0 & 0 \\0 & 0\end{matrix} & {- {\underset{\_}{U}}_{L}}\end{matrix}\end{matrix} \\\begin{matrix}{- \left( {\underset{\_}{U}}_{L}^{\prime} \right)^{T}} & {{- \left( {\underset{\_}{U}}_{L}^{\prime} \right)^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (44) \\{{\underset{\_}{\Theta}}_{6}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 \\0 & 0 \\0 & 0 \\0 & 0\end{matrix} & \begin{matrix}0 \\0 \\\left( {{\underset{\_}{U}}_{L} - \underset{\_}{U_{W}}} \right)\end{matrix}\end{matrix} \\\begin{matrix}{- \left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}} & {\left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (45) \\{{\underset{\_}{\Theta}}_{7}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 \\0 & 0 \\0 & 0 \\0 & 0\end{matrix} & \begin{matrix}0 \\0 \\{- {\underset{\_}{U}}_{L}}\end{matrix}\end{matrix} \\\begin{matrix}\left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T} & {{- \left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (46) \\{{\underset{\_}{\Theta}}_{8}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 \\0 & 0 \\0 & 0 \\0 & 0\end{matrix} & \begin{matrix}0 \\0 \\\left( {{\underset{\_}{U}}_{L} + \underset{\_}{U_{W}}} \right)\end{matrix}\end{matrix} \\\begin{matrix}{- \left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}} & {{- \left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}}\underset{\_}{C}} \\{- \left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)} & {{- \left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (47) \\{{{\underset{\_}{\Theta}}_{9}^{\prime} = {2\begin{bmatrix}\begin{matrix}\begin{matrix}0 & 0 \\0 & 0 \\0 & 0 \\0 & 0\end{matrix} & \begin{matrix}0 \\0 \\{- \left( {{\underset{\_}{U}}_{L} + \underset{\_}{U_{W}}} \right)}\end{matrix}\end{matrix} \\\begin{matrix}{- \left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}} & {{- \left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}}{{where}\text{:}}} & (48) \\{{\underset{\_}{U}}_{L}^{T} = \begin{bmatrix}{\cos \; \Theta} & {{- \sin}\; \Theta}\end{bmatrix}} & (49) \\{{\underset{\_}{U}}_{W}^{T} = \begin{bmatrix}{\sin \; \Theta} & {\cos \; \Theta}\end{bmatrix}} & (50) \\{\left( {\underset{\_}{U}}_{L}^{T} \right)^{\prime} = {- \begin{bmatrix}{\sin \; \Theta} & {\cos \; \Theta}\end{bmatrix}}} & (51) \\{\left( {\underset{\_}{U}}_{W}^{T} \right)^{\prime} = \begin{bmatrix}{\cos \; \Theta} & {{- \sin}\; \Theta}\end{bmatrix}} & (52) \\{{\left( {\underset{\_}{U}}_{L}^{T} \right)^{\prime} = {- {\underset{\_}{U}}_{W}^{T}}},{\left( {\underset{\_}{U}}_{W}^{T} \right)^{\prime} = {\underset{\_}{U}}_{L}^{T}}} & (53)\end{matrix}$

and the differences dΘ _(j)′=Θ ₁′−Θ _(j)′; calculated as:

$\begin{matrix}\begin{matrix}{{d\; {\underset{\_}{\underset{\_}{\Theta}}}_{2}^{\prime}} = {2\begin{bmatrix}\begin{matrix}0 & 0 & 0 \\0 & 0 & {1/2} \\0 & 0 & {\underset{\_}{- U}}_{W} \\0 & 0 & \;\end{matrix} \\\begin{matrix}{- {\underset{\_}{U}}_{L}^{T}} & {{\underset{\_}{- U}}_{L}^{T}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & {{\underset{\_}{d\; \Theta}}_{3}^{\prime} = {2\begin{bmatrix}\begin{matrix}0 & 0 & 0 \\0 & 0 & {1/2} \\0 & 0 & {\underset{\_}{U}}_{W} \\0 & 0 & \;\end{matrix} \\\begin{matrix}{- {\underset{\_}{U}}_{L}^{T}} & {{- {\underset{\_}{U}}_{L}^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}}\end{matrix} & (54) \\\begin{matrix}{{d\; {\underset{\_}{\underset{\_}{\Theta}}}_{4}^{\prime}} = {2\begin{bmatrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & 0 \\0 & 0 & {\underset{\_}{- U}}_{L} \\0 & 0 & \;\end{matrix} \\\begin{matrix}{\underset{\_}{- U}}_{W}^{T} & {{\underset{\_}{U}}_{W}^{T}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & {{\underset{\_}{d\; \Theta}}_{5}^{\prime} = {2\begin{bmatrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & 0 \\0 & 0 & {\underset{\_}{U}}_{L}\end{matrix} \\\begin{matrix}{\underset{\_}{U}}_{W}^{T} & {{- {\underset{\_}{U}}_{W}^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}}\end{matrix} & (55) \\{{d\; {\underset{\_}{\underset{\_}{\Theta}}}_{6}^{\prime}} = {2\begin{bmatrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & {1/2} \\0 & 0 & {- \left( {{\underset{\_}{U}}_{L} - \underset{\_}{U_{W}}} \right)} \\0 & 0 & \;\end{matrix} \\\begin{matrix}{- \left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}} & {\left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (56) \\{{d\; {\underset{\_}{\underset{\_}{\Theta}}}_{7}^{\prime}} = {2\begin{bmatrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & {1/2} \\0 & 0 & \left( {{\underset{\_}{U}}_{L} - \underset{\_}{U_{W}}} \right) \\0 & 0 & \;\end{matrix} \\\begin{matrix}{- \left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}} & {\left( {{\underset{\_}{U}}_{L}^{\prime} + {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (57) \\{{d\; {\underset{\_}{\underset{\_}{\Theta}}}_{8}^{\prime}} = {2\begin{bmatrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & {1/2} \\0 & 0 & {- \left( {{\underset{\_}{U}}_{L} + \underset{\_}{U_{W}}} \right)} \\0 & 0 & \;\end{matrix} \\\begin{matrix}\left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T} & {{- \left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (58) \\{{d\; {\underset{\_}{\underset{\_}{\Theta}}}_{9}^{\prime}} = {2\begin{bmatrix}\begin{matrix}0 & 0 & {1/2} \\0 & 0 & {1/2} \\0 & 0 & \left( {{\underset{\_}{U}}_{L} + \underset{\_}{U_{W}}} \right) \\0 & 0 & \;\end{matrix} \\\begin{matrix}{- \left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}} & {{- \left( {{\underset{\_}{U}}_{L}^{\prime} - {\underset{\_}{U}}_{W}^{\prime}} \right)^{T}}\underset{\_}{C}}\end{matrix}\end{bmatrix}}} & (59)\end{matrix}$

In summary, the log-likelihood function to be maximized is:

L=ln [p(B|Z _(1:k-1))]+Σ_(l∈IN) ln(Φ₁)+Σ_(l∈OUT) ln(1−Φ_(l))  (60)

and its gradient is:

$\begin{matrix}{{\frac{\partial\underset{\_}{L}}{\partial\underset{\_}{M}} = {{\frac{\partial}{\partial\underset{\_}{M}}{\ln \left\lbrack {p\left( \underset{\_}{B} \middle| {\underset{\_}{Z}}_{1:{k - 1}} \right)} \right\rbrack}} + {\sum\limits_{l \in {IN}}{\ln \left( \varphi_{l} \right)}} + {\sum\limits_{l \in {OUT}}{\ln \left( {1 - \varphi_{l}} \right)}}}}{{where}\text{:}}} & (61) \\{{{{\underset{\_}{\underset{\_}{\alpha}}}_{l} = \frac{\sum\limits_{j = 2}^{9}{d{{\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime} \cdot {\exp \left( {\lambda {\underset{\_}{\Theta}}_{j}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}}}}{\sum\limits_{j = 1}^{9}{\exp \left( {\lambda {\underset{\_}{\Theta}}_{j}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}}},{{\overset{\sim}{\underset{\_}{Z}}}_{l} = \begin{bmatrix}{\underset{\_}{Z}}_{l} \\1\end{bmatrix}}}{{and}\text{:}}} & (62) \\{{\Phi_{l} = \frac{\exp \left( {\lambda {\underset{\_}{\Theta}}_{n}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}{\sum\limits_{i = 1}^{9}{\exp \left( {\lambda \; {\underset{\_}{\Theta}}_{i}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}}},{{d\; {\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}} = \begin{matrix}{{{matrices}\mspace{14mu} A\; 49} - {A\; 54}} \\{\forall{j \in \left\lbrack {2,\ldots \mspace{14mu},9} \right\rbrack}}\end{matrix}}} & (63)\end{matrix}$

Now, λ is a tuning parameter directly proportional to the “sharpness” ofthe softmax function, where:

${\Phi_{l} = {{measured}\mspace{14mu} {probability}\mspace{14mu} {that}\mspace{14mu} a\mspace{14mu} {point}\mspace{14mu} \underset{\_}{did}\mspace{14mu} {originate}\mspace{14mu} {from}\mspace{14mu} {an}\mspace{14mu} {object}}},{{1 - \Phi_{l}} = {{measured}\mspace{14mu} {probability}\mspace{14mu} {that}\mspace{14mu} a\mspace{14mu} {point}\mspace{14mu} \underset{\_}{{did}\mspace{14mu} {not}}\mspace{14mu} {originate}\mspace{14mu} {from}\mspace{14mu} {object}}},\mspace{20mu} {and}$$\frac{\Phi_{l}}{1 - \Phi_{l}} = {\frac{\exp \left( {\lambda \; {\underset{\_}{\Theta}}_{n}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}{\sum\limits_{i = 2}^{9}{\exp \left( {\lambda \; {\underset{\_}{\Theta}}_{i}^{T}{\overset{\sim}{\underset{\_}{Z}}}_{l}} \right)}} = {{Probability}\mspace{14mu} {ratio}\mspace{14mu} {of}\mspace{14mu} {point}\mspace{14mu} {in}\text{/}{out}\mspace{14mu} {of}\mspace{14mu} {an}\mspace{14mu} {{object}.}}}$

The gradient can be used to maximize the log-likelihood function byfinding the Hessian:

$\begin{matrix}{{\underset{\_}{\underset{\_}{H}} = {\frac{\partial^{2}L}{\partial{\underset{\_}{M}}^{2}} = {{\frac{\partial^{2}}{\partial{\underset{\_}{M}}^{2}}L_{B}} + {\lambda\left\lbrack {\underset{\underset{{\underset{\_}{\underset{\_}{H}}}_{l}^{IN}}{}}{\sum\limits_{l \in {IN}}{\frac{\partial}{\partial\underset{\_}{M}} \cdot {\underset{\_}{J}}_{l}^{IN}}} + \underset{\underset{{\underset{\_}{\underset{\_}{H}}}_{l}^{OUT}}{}}{\sum\limits_{l \in {OUT}}{\frac{\partial}{\partial\underset{\_}{M}} \cdot \frac{\Phi_{l}}{1 - \Phi_{l}} \cdot {\underset{\_}{J}}_{l}^{IN}}}} \right\rbrack}}}}\text{}\mspace{20mu} {{where}:}} & (64) \\{\mspace{79mu} {{{\underset{\_}{J}}_{l}^{IN} = {{\underset{\_}{\underset{\_}{\alpha}}}_{l} \cdot {\overset{\sim}{\underset{\_}{Z}}}_{l}}},{{\underset{\_}{\underset{\_}{H}}}_{l}^{IN} = {\frac{\partial}{\partial\underset{\_}{M}} \cdot {\underset{\_}{J}}_{l}^{IN}}},{{\underset{\_}{\underset{\_}{H}}}_{l}^{OUT} = {\frac{d}{d\; \underset{\_}{M}} \cdot \frac{\Phi_{l}}{1 - \Phi_{l}} \cdot {\underset{\_}{J}}_{l}^{IN}}}}} & (65)\end{matrix}$

Using the Product Rule to express H_(l) ^(OUT) in denominator format:

$\begin{matrix}{{\underset{\_}{\underset{\_}{H}}}_{l}^{OUT} = {{\frac{\Phi_{l}}{1 - \Phi_{l}} \cdot {\underset{\_}{\underset{\_}{H}}}_{l}^{IN}} + {\left\lbrack {\frac{d}{d\; \underset{\_}{M}} \cdot \frac{\Phi_{l}}{1 - \Phi_{l}}} \right\rbrack \left( {\underset{\_}{J}}_{l}^{IN} \right)^{T}\ldots}}} & (66)\end{matrix}$

Now use the Quotient Rule:

$\begin{matrix}{{\frac{d}{d\underset{\_}{M}} \cdot \frac{\Phi_{l}}{1 - \Phi_{l}}} = \frac{{\left\lbrack {\frac{d}{d\; \underset{\_}{M}} \cdot \Phi_{l}} \right\rbrack \left( {1 - \Phi_{l}} \right)} - {{\Phi_{l} \cdot \frac{d}{d\; \underset{\_}{M}}}\left( {1 - \Phi_{l}} \right)}}{\left( {1 - \Phi_{l}} \right)^{2}}} & (67) \\{= {\frac{\frac{d}{d\; \underset{\_}{M}} \cdot \Phi_{l}}{\left( {1 - \Phi_{l}} \right)^{2}} = {\lambda \; {\frac{\Phi_{l}}{\left( {1 - \Phi_{l}} \right)^{2}} \cdot \underset{\underset{= {\underset{\_}{J}}_{l}^{IN}}{}}{{\underset{\_}{\underset{\_}{\alpha}}}_{l} \cdot {\overset{\sim}{\underset{\_}{Z}}}_{l}}}}}} & (68)\end{matrix}$

substitute back into (66):

$\begin{matrix}{{{\underset{\_}{\underset{\_}{H}}}_{l}^{OUT} = {{\frac{\Phi_{l}}{1 - \Phi_{l}} \cdot {\underset{\_}{\underset{\_}{H}}}_{l}^{IN}} + {\lambda \cdot \frac{\Phi_{l}}{\left( {1 - \Phi_{l}} \right)^{2}} \cdot {\underset{\_}{J}}_{l}^{IN} \cdot \left( {\underset{\_}{J}}_{l}^{IN} \right)^{T}}}}{and}} & (69) \\{{\underset{\_}{\underset{\_}{H}}}_{l}^{IN} = {{\frac{\partial}{\partial\underset{\_}{M}}{\underset{\_}{J}}_{l}^{IN}} = {{\frac{\partial}{\partial\underset{\_}{M}}{\underset{\_}{\underset{\_}{\alpha}}}_{l}{\overset{\sim}{\underset{\_}{Z}}}_{l}} = {\frac{\partial}{\partial\underset{\_}{M}}\frac{\sum\limits_{j = 2}^{9}{{\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}f_{j}}}{\sum\limits_{j = 1}^{9}f_{j}}{\overset{\sim}{\underset{\_}{Z}}}_{l}}}}} & (70)\end{matrix}$

which, by the quotient rule equals:

$\begin{matrix}{{= \frac{\begin{matrix}{{\left\lbrack {\sum\limits_{j = 2}^{9}{\frac{d}{d\; \underset{\_}{M}}{\underset{\_}{\overset{\_}{\underset{\_}{\Phi}}}}_{j}^{\prime}f_{j}{\overset{\sim}{\underset{\_}{Z}}}_{l}}} \right\rbrack \cdot {\sum\limits_{j = 1}^{9}f_{j}}} - {\left\lbrack {\sum\limits_{j = 1}^{9}{\frac{d}{d\; \underset{\_}{M}}f_{j}}} \right\rbrack \cdot}} \\\left\lbrack {\sum\limits_{j = 2}^{9}{\frac{d}{d\; \underset{\_}{M}}{\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{l}^{\prime}f_{j}{\overset{\sim}{\underset{\_}{Z}}}_{l}}} \right\rbrack\end{matrix}}{\left\lbrack {\sum\limits_{j = 1}^{9}f_{j}} \right\rbrack^{2}}}{{where}:}} & (71) \\{{\frac{d}{d\; \underset{\_}{M}}f_{j}} = {\lambda \; f_{j}{\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{l}^{\prime}{\overset{\sim}{\underset{\_}{Z}}}_{l}}} & (72)\end{matrix}$

which can be substituted into (71):

$\begin{matrix}{{= {\frac{\left\lbrack {\sum\limits_{j = 2}^{9}{\frac{d}{d\; \underset{\_}{M}}{\underset{\_}{\overset{\_}{\underset{\_}{\Theta}}}}_{j}^{\prime}f_{j}{\overset{\sim}{\underset{\_}{Z}}}_{l}}} \right\rbrack}{\sum\limits_{j = 1}^{9}f_{j}} - {{\lambda\left( \frac{\sum\limits_{j = 1}^{9}{{\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}f_{j}}}{\sum\limits_{j = 1}^{9}f_{j}} \right)}{\overset{\sim}{\underset{\_}{Z}}}_{l}{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}{\underset{\_}{\underset{\_}{\alpha}}}_{l}^{T}}}}{and}} & (73) \\{{= {\frac{\left\lbrack {\sum\limits_{j = 2}^{9}{\frac{d}{d\; \underset{\_}{M}}{\underset{\_}{\overset{\_}{\underset{\_}{\Theta}}}}_{j}^{\prime}{\overset{\sim}{\underset{\_}{Z}}}_{l}f_{j}}} \right\rbrack}{\sum\limits_{j = 1}^{9}f_{j}} - {{\lambda\left( {{\underset{\_}{\underset{\_}{\alpha}}}_{l} - {\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}} \right)}{\overset{\sim}{\underset{\_}{Z}}}_{l}{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}{\underset{\_}{\underset{\_}{\alpha}}}_{l}^{T}}}}{{where}:}} & (74) \\{{\frac{d}{d\; \underset{\_}{M}}{\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{\prime}{\overset{\sim}{\underset{\_}{Z}}}_{l}f_{j}} = {\underset{\underset{= {{\sum\limits_{i = 1}^{3}{\frac{\partial\; {{\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{\prime}{({:{,i}})}}}{d\; \underset{\_}{M}}{\overset{\sim}{\underset{\_}{Z}}}_{j}}} = {\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{''}}}{}}{f_{j}\frac{d}{d\; \underset{\_}{M}}{\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{\prime}{\overset{\sim}{\underset{\_}{Z}}}_{l}} + {\frac{{df}_{j}}{d\; \underset{\_}{M}}{{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}\left( {\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{\prime} \right)}^{T}}}} & (75)\end{matrix}$

which can be substituted back into (74):

$\begin{matrix}{= {\frac{\sum\limits_{j = 2}^{9}\left\lbrack {{f_{j}{\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{''}} + {\lambda \; f_{j}{\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}{\overset{\sim}{\underset{\_}{Z}}}_{l}{{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}\left( {\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{\prime} \right)}^{T}}} \right\rbrack}{\sum\limits_{j = 1}^{9}f_{j}} + {{\lambda \left( {{\underset{\_}{\underset{\_}{\alpha}}}_{l} - {\underset{\_}{\underset{\_}{\Theta}}}_{j}^{\prime}} \right)}{\overset{\sim}{\underset{\_}{Z}}}_{l}{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}{\underset{\_}{\underset{\_}{\alpha}}}_{l}^{T}}}} & (76)\end{matrix}$

and substituting back into (70):

$\begin{matrix}{{{\underset{\_}{\underset{\_}{H}}}_{l}^{IN} = {\frac{\sum\limits_{j = 2}^{9}{{\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{''}f_{j}}}{\sum\limits_{j = 1}^{9}f_{j}} + {\lambda \left\{ {\frac{\sum\limits_{j = 2}^{9}{{\underset{\_}{\underset{\_}{\Theta}}}_{j}^{''}{\overset{\sim}{\underset{\_}{Z}}}_{l}{{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}\left( {\underset{\_}{\underset{\_}{\overset{\_}{\Theta}}}}_{j}^{\prime} \right)}^{T}f_{j}}}{\sum\limits_{j = 1}^{9}f_{j}} + \underset{\underset{\begin{matrix}{= {{{\underset{\_}{J}}_{l}^{IN} \cdot {({\underset{\_}{J}}_{l}^{IN})}^{T}} - {{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{\prime}{{\overset{\sim}{\underset{\_}{Z}}}_{l}{({\underset{\_}{J}}_{l}^{IN})}}^{T}}}} \\{= {{({{\underset{\_}{J}}_{l}^{IN} - {{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{\prime}{\overset{\sim}{\underset{\_}{Z}}}_{l}}})}{({\underset{\_}{J}}_{l}^{IN})}^{T}}}\end{matrix}}{}}{\left( {{\underset{\_}{\underset{\_}{\alpha}}}_{l} - {\underset{\_}{\underset{\_}{\Theta}}}_{1}^{\prime}} \right){\overset{\sim}{\underset{\_}{Z}}}_{l}{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{\prime}}} \right\}}}}\mspace{20mu} {{yields}:}} & (77) \\{{\underset{\_}{\underset{\_}{H}}}_{l}^{IN} = {\frac{\sum\limits_{j = 2}^{9}\left( {{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{''} - {\underset{\_}{\underset{\_}{\Theta}}}_{j}^{''}} \right)}{\sum\limits_{j = 1}^{9}f_{j}} + {\lambda \left\{ {\frac{\sum\limits_{j = 2}^{9}{f_{j}{\underset{\_}{\underset{\_}{\Theta}}}_{j}^{''}{\overset{\sim}{\underset{\_}{Z}}}_{l}{{\overset{\sim}{\underset{\_}{Z}}}_{l}^{T}\left( {{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{''} - {\underset{\_}{\underset{\_}{\Theta}}}_{j}^{''}} \right)}^{T}}}{\sum\limits_{j = 1}^{9}f_{j}}\left( {{\underset{\_}{J}}_{l}^{IN} - {{\underset{\_}{\underset{\_}{\Theta}}}_{1}^{\prime}{\overset{\sim}{\underset{\_}{Z}}}_{l}}} \right)\left( {\underset{\_}{J}}_{l}^{IN} \right)^{T}} \right\}}}} & (78)\end{matrix}$

which is the desired Hessian form. The Θ _(l)′ can be calculated as:

$\begin{matrix}{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{1} = {\underset{\_}{O}}_{5 \times 5}} & (79) \\\begin{matrix}{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{2} = {2\begin{bmatrix}{\underset{\_}{\underset{\_}{O}}}_{4 \times 4} & {\underset{\_}{O}}_{2 \times 1} \\\; & {\underset{\_}{U}}_{L} \\{{\underset{\_}{O}}_{1 \times 2}{\underset{\_}{U}}_{L}^{T}} & {{\underset{\_}{U}}_{W}^{T}\left( {{\underset{\_}{Z}}_{l} - \underset{\_}{C}} \right)}\end{bmatrix}}} & \begin{matrix}{{\underset{\_}{U}}_{L} = \begin{bmatrix}{\cos \; \Theta} \\{{- \sin}\; \Theta}\end{bmatrix}} \\{{\underset{\_}{U}}_{W} = \begin{bmatrix}{\sin \; \Theta} \\{\cos \; \Theta}\end{bmatrix}}\end{matrix}\end{matrix} & (80) \\{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{3} = {- {{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{2}}} & (81) \\{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{4} = {2\begin{bmatrix}{\underset{\_}{\underset{\_}{O}}}_{4 \times 4} & {\underset{\_}{O}}_{2 \times 1} \\\; & {\underset{\_}{- U}}_{W} \\{{\underset{\_}{O}}_{1 \times 2} - {\underset{\_}{U}}_{W}} & {{\underset{\_}{U}}_{W}^{T}\left( {{\underset{\_}{Z}}_{l} - \underset{\_}{C}} \right)}\end{bmatrix}}} & (82) \\{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{5} = {- {{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{4}}} & (83) \\{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{6} = {2\begin{bmatrix}{\underset{\_}{\underset{\_}{O}}}_{4 \times 4} & {\underset{\_}{O}}_{2 \times 1} \\\; & {- \left( {{\underset{\_}{U}}_{L} + {\underset{\_}{U}}_{W}} \right)} \\{{\underset{\_}{O}}_{1 \times 2} - \left( {{\underset{\_}{U}}_{L} + {\underset{\_}{U}}_{W}} \right)^{T}} & {\left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)^{T}\left( {{\underset{\_}{Z}}_{l} - \underset{\_}{C}} \right)}\end{bmatrix}}} & (84) \\{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{7} = {- {{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{6}}} & (85) \\{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{8} = {2\begin{bmatrix}{\underset{\_}{\underset{\_}{O}}}_{4 \times 4} & {\underset{\_}{O}}_{2 \times 1} \\\; & {- \left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)} \\{{\underset{\_}{O}}_{1 \times 2}\left( {{\underset{\_}{U}}_{L} - {\underset{\_}{U}}_{W}} \right)}^{T} & {\left( {{\underset{\_}{U}}_{L} + {\underset{\_}{U}}_{W}} \right)^{T}\left( {{\underset{\_}{Z}}_{l} - \underset{\_}{C}} \right)}\end{bmatrix}}} & (86) \\{{{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{7} = {- {{\underset{\_}{\underset{\_}{\Theta}}}_{l}^{''}}_{6}}} & (87)\end{matrix}$

Computing devices such as those discussed herein generally each includeinstructions executable by one or more computing devices such as thoseidentified above, and for carrying out blocks or steps of processesdescribed above. For example, process blocks discussed above may beembodied as computer-executable instructions.

Computer-executable instructions may be compiled or interpreted fromcomputer programs created using a variety of programming languagesand/or technologies, including, without limitation, and either alone orin combination, Java™, C, C++, Visual Basic, Java Script, Perl, HTML,etc. In general, a processor (e.g., a microprocessor) receivesinstructions, e.g., from a memory, a computer-readable medium, etc., andexecutes these instructions, thereby performing one or more processes,including one or more of the processes described herein. Suchinstructions and other data may be stored in files and transmitted usinga variety of computer-readable media. A file in a computing device isgenerally a collection of data stored on a computer readable medium,such as a storage medium, a random access memory, etc.

A computer-readable medium includes any medium that participates inproviding data (e.g., instructions), which may be read by a computer.Such a medium may take many forms, including, but not limited to,non-volatile media, volatile media, etc. Non-volatile media include, forexample, optical or magnetic disks and other persistent memory. Volatilemedia include dynamic random access memory (DRAM), which typicallyconstitutes a main memory. Common forms of computer-readable mediainclude, for example, a floppy disk, a flexible disk, hard disk,magnetic tape, any other magnetic medium, a CD-ROM, DVD, any otheroptical medium, punch cards, paper tape, any other physical medium withpatterns of holes, a RAM, a PROM, an EPROM, a FLASH-EEPROM, any othermemory chip or cartridge, or any other medium from which a computer canread.

All terms used in the claims are intended to be given their plain andordinary meanings as understood by those skilled in the art unless anexplicit indication to the contrary in made herein. In particular, useof the singular articles such as “a,” “the,” “said,” etc. should be readto recite one or more of the indicated elements unless a claim recitesan explicit limitation to the contrary.

The term “exemplary” is used herein in the sense of signifying anexample, e.g., a reference to an “exemplary widget” should be read assimply referring to an example of a widget.

The adverb “approximately” modifying a value or result means that ashape, structure, measurement, value, determination, calculation, etc.may deviate from an exact described geometry, distance, measurement,value, determination, calculation, etc., because of imperfections inmaterials, machining, manufacturing, sensor measurements, computations,processing time, communications time, etc.

In the drawings, the same reference numbers indicate the same elements.Further, some or all of these elements could be changed. With regard tothe media, processes, systems, methods, etc. described herein, it shouldbe understood that, although the steps of such processes, etc. have beendescribed as occurring according to a certain ordered sequence, suchprocesses could be practiced with the described steps performed in anorder other than the order described herein. It further should beunderstood that certain steps could be performed simultaneously, thatother steps could be added, or that certain steps described herein couldbe omitted. In other words, the descriptions of processes herein areprovided for the purpose of illustrating certain embodiments, and shouldin no way be construed so as to limit the claimed invention

We claim:
 1. A method, comprising: determining an object based on afirst plurality of 3D data points by determining a first rectangularbound centered on the object based on joint probabilities; determiningthe object in a second plurality of 3D data points by determining asecond rectangular bound centered on the object based on the firstrectangular bound and joint probabilities; and piloting a vehicle basedon the object.
 2. The method of claim 1, wherein the 3D data points aredetermined by LIDAR.
 3. The method of claim 1, wherein determining theobject includes determining location, pose and size of the firstrectangular bound and the second rectangular bound.
 4. The method ofclaim 1, wherein determining the object includes comparing the first 3Ddata points to previously acquired 3D data points to determine objectdata points and background data points.
 5. The method of claim 4 whereinthe first rectangular bound and the second rectangular bound aredetermined by maximizing the joint probabilities of the object datapoints with respect to the background data points.
 6. The method ofclaim 5, wherein maximizing the joint probabilities determines a 3×3grid centered on the object data points, wherein a center grid of the3×3 grid includes one or more object points and no background datapoints.
 7. The method of claim 6, wherein the first rectangular bound isdefined as the center grid of the 3×3 grid and is used as a startingpoint for determining the second rectangular bound based on the jointprobabilities.
 8. The method of claim 7, wherein the joint probabilitiesare Bayesian probabilities wherein a probability that an object datapoint belongs to the object is conditioned on a probability that theobject data point belonged to the object at a previous time step.
 9. Themethod of claim 8, wherein the joint probabilities are determined bymaximizing natural logarithms of the joint probabilities.
 10. The methodof claim 9, wherein the joint probabilities are maximized by determininggradients of natural logarithms of each of the joint probabilities. 11.An apparatus, comprising: a processor; a memory, the memory storinginstructions executable by the processor to: determine an object basedon a first plurality of 3D data points by determining a firstrectangular bound centered on the object based on joint probabilities;determine the object in a second plurality of 3D data points bydetermining a second rectangular bound centered on the object based onthe first rectangular bound and joint probabilities; and pilot a vehiclebased on the object.
 12. The apparatus of claim 11, wherein the 3D datapoints are determined by LIDAR.
 13. The apparatus of claim 11, whereindetermine the object includes determining location, pose and size of thefirst rectangular bound and the second rectangular bound.
 14. Theapparatus of claim 11, wherein determine the object includes comparingthe first 3D data points to previously acquired 3D data points todetermine object data points and background data points.
 15. Theapparatus of claim 14, wherein the first rectangular bound and thesecond rectangular bound are determined by maximizing the jointprobabilities of the object data points with respect to the backgrounddata points.
 16. The apparatus of claim 15, wherein maximizing the jointprobabilities determines a 3×3 grid centered on the object data points,wherein a center grid of the 3×3 grid includes one or more object pointsand no background data points.
 17. The apparatus of claim 16, whereinthe first rectangular bound is defined as the center grid of the 3×3grid and is used as a starting point for determining the secondrectangular bound based on the joint probabilities.
 18. The apparatus ofclaim 17, wherein the joint probabilities are Bayesian probabilitieswherein a probability that an object data point belongs to the object isconditioned on a probability that the object data point belonged to theobject at a previous time step.
 19. The apparatus of claim 18, whereinthe joint probabilities are determined by maximizing natural logarithmsof the joint probabilities.
 20. The apparatus of claim 19, wherein thejoint probabilities are maximized by determining gradients of naturallogarithms of each of the joint probabilities.